/**
 * @file ArmorBase.h
 * @brief
 * @author LiuZhihao (2488288779@qq.com)
 * @date 2023-12-10
 *
 * @copyright Copyright (C) 2023, HITCRT_VISION, all rights reserved.
 *
 * @par 修改日志:
 * <table>
 * <tr><th>Date       <th>Author  <th>Description
 * <tr><td>2023-12-10 <td>LiuZhihao  <td>
 * </table>
 */

#pragma once

#include <Eigen/Core>
#include <Eigen/Dense>
#include <cmath>
#include <ctime>
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>

#include "../basic/Basic.h"

namespace hitcrt {
/**
 * @brief 灯条
 * @author BG2EDG (928330305@qq.com)
 */
class LightBar {
   public:
    LightBar() {}
    ~LightBar() {}
    float m_length;
    float m_width;
    float m_contourArea;  // 轮廓面积
    float m_colorRatio;   // 颜色比例
    cv::Point2f m_directVec;
    cv::Point2f m_directUnitVec;
    cv::Rect m_boundingRect;
    cv::RotatedRect m_rotatedRect;
    cv::Point2f m_top;     // 上端点
    cv::Point2f m_bottom;  // 下端点
    cv::Point2f m_center;  // 中心点
    void setParam(const float length, const float width,
                  const float contourArea, const float colorRatio,
                  const cv::Point2f directVec,
                  const std::vector<cv::Point>& contour,
                  const std::vector<cv::Point2f>& insidePoints,
                  const cv::RotatedRect rotatedRect, const cv::Point2f top,
                  const cv::Point2f bottom, const cv::Point2f center);
};

/**
 * @brief 装甲板
 * @author BG2EDG (928330305@qq.com)
 */
class Armor {
   public:
    Armor() {

    };
    Armor(const LightBar& l1, const LightBar& l2, const Pattern pattern) {
        m_product = std::fabs(l1.m_directVec.x * l2.m_directVec.y -
                              l1.m_directVec.y * l2.m_directVec.x);

        if (l1.m_center.x < l2.m_center.x) {
            m_topLeft = l1.m_top;
            m_bottomLeft = l1.m_bottom;
            m_centerLeft = l1.m_center;

            m_topRight = l2.m_top;
            m_bottomRight = l2.m_bottom;
            m_centerRight = l2.m_center;
        } else {
            m_topLeft = l2.m_top;
            m_bottomLeft = l2.m_bottom;
            m_centerLeft = l2.m_center;

            m_topRight = l1.m_top;
            m_bottomRight = l1.m_bottom;
            m_centerRight = l1.m_center;
        }

        m_centerUV =
            (m_topLeft + m_bottomLeft + m_topRight + m_bottomRight) / 4.0;
        m_height = cv::min(l1.m_length, l2.m_length);
        m_width = cv::norm(m_centerLeft - m_centerRight);
        m_aspectRatio = m_width / m_height;  // 宽比高
        m_pattern = pattern;
        // m_pattern = Pattern::UNKNOWN;  // 利用灯条获取装甲板时类别未知
    }
    // Raw from Detect (Both NN & Tradition)
    // 装甲板特征点
    cv::Point2f m_topLeft;
    cv::Point2f m_topRight;
    cv::Point2f m_centerLeft;
    cv::Point2f m_centerRight;
    cv::Point2f m_bottomLeft;
    cv::Point2f m_bottomRight;
    cv::Point2f m_centerUV;      // 中心点在图像中的位置
    cv::Point2f m_centerUVFilt;  // 中心点在图像中的位置
    cv::Point2f m_carCenterUV;
    cv::Point2f m_filtCenterUV;
    cv::Point2f m_filtCenterUVCar;
    cv::Point2f m_predictPosUV;
    cv::Point2f m_calcPosUV;
    cv::Point2f m_filtPosUV;

    cv::Point3f m_filterPointmeasure; //自己定义的存储滤波后的位置
    // 数字编号
    Pattern m_pattern = Pattern::UNKNOWN;
    // int m_pattern = 0;

    // Raw from recv/system
    TimePoint m_timeStamp;
    double m_time = 0;  /// 获取时间，等于抓图时间


    double m_yawSend;
    double m_pitchSend;

    float m_currPitchRAD = 0.0, m_currYawRAD = 0.0,
          m_currRollRAD = 0.0;  // pithc&yaw当前值

    // Calculate
    float m_width;
    float m_height;
    float m_aspectRatio;  // 宽比高，与屏幕16:9,4:3等定义方式一致
    float m_product = 0;  // 灯条平行度
    Size m_size;          // 定义一个TYPE型的变量
    double m_horizontalDistance = 0.0;  // 装甲到云台坐标系原点的水平面投影距离

    // Ttj
    float m_dislikeValue;          // 决策
    std::vector<float> m_weights;  // 轨迹匹配权重
    // double m_lastR;                // 小陀螺时相邻装甲板数据
    // double m_lastY;

    // 与图无关的矩阵换成Eigen矩阵，计算中间变量用double
    Eigen::MatrixXd m_rotVec, m_transVec;
    Eigen::MatrixXd m_rotVec1;
    double m_yawToC, m_yawToR, m_pitchToR, m_rollToR, m_yawToR1;
    double m_yawToRraw;
    std::vector<float> m_reprojectionErrorVector;  // 记录两个解对应的重投影误差
    Eigen::MatrixXd m_pointCH, m_pointGH, m_pointG3, m_pointR3;
    Eigen::Matrix3d m_rotRoll, m_rotYaw, m_rotPitch;
    Eigen::MatrixXd m_R2G, m_G2C;

    Eigen::MatrixXd m_filtPos;                        // xyz滤波值，单位m
    cv::Point3d m_predictXYZ;                         // xyz预测值，单位m
    cv::Point3d m_measurePoint;                       // xyz坐标系转换的值
    double m_calcPitchRAD = 0.0, m_calcYawRAD = 0.0;  // pitch&yaw计算值
    double m_filtPitchDEG = 0.0, m_filtYawDEG = 0.0;  // pitch&yaw滤波值
};

}  // namespace hitcrt
